﻿/*============================================================================
 * AUTH: Matthew Baum, 9753095
 * DESC: Class definitions for collision detection and collision resolution
 *============================================================================*/ 

// Using directives
using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;

namespace RisingThreat
{
    // Class used to detect "real"collisions from a set of potential collisions
    class CollisionDetector
    {
        // Constructor
        public CollisionDetector()
        {
        }

        // Detect NPC/LEVEL collisions
        public List<QuadTreeItem> detectLevelCollisions(BoundingSphere boundingsphere, HashSet<QuadTreeItem> potentialCollisions)
        {
            // Create a list to hold colliding items
            List<QuadTreeItem> collisions = new List<QuadTreeItem>();

            // Iterate through the set of potential collisions
            foreach (QuadTreeItem item in potentialCollisions)
            {
                // Test for a true collision
                if (boundingsphere.Intersects(item.getBoundingVolume()))
                {
                    // Add to list
                    collisions.Add(item);
                }
            }

            // Return the collisions
            return collisions;
        }
    }

    /*
    // Class used to resolve collisions
    class CollisionResolver
    {
        // Constructor
        public CollisionResolver()
        {
        }

        // Detect NPC/LEVEL collisions
        public void resolveLevelCollisions(CollisionHandler npc, List<QuadTreeItem> collisions)
        {
            // Resolve each collision in turn
            foreach (QuadTreeItem item in collisions)
            {
                // Make sure objects are still colliding
                if (npc.collides(item))
                {
                    resolveNPCLevelCollision(npc, item);
                }
            }
        }

    */

        /*================================================================= 
         * Resolve a single level collision
         * NOTE: Walls have infinite mass and do not move.
         *=================================================================*/
        /*
        private void resolveNPCLevelCollision(CollisionHandler npc, QuadTreeItem levelItem)
        {
            // Get planes from level object
            Plane[] planes = levelItem.getFaces();

            // Get NPC bounding sphere
            BoundingSphere sphere = npc.getBoundingVolume();

            // Try colliding with each face
            if(planes[0].Intersects(sphere).HasFlag(PlaneIntersectionType.Intersecting))
            {
                        resolveNpcPlaneCollision(npc, planes[0]);
            }
                
                //Console.WriteLine("FRONT");
            else if(planes[1].Intersects(sphere).HasFlag(PlaneIntersectionType.Intersecting))
            {
              
                    resolveNpcPlaneCollision(npc, planes[1]);
                //Console.WriteLine("BACK");
            }
            else if(planes[2].Intersects(sphere).HasFlag(PlaneIntersectionType.Intersecting))
            {
                    resolveNpcPlaneCollision(npc, planes[2]);
                //Console.WriteLine("LEFT");
            }
            else if(planes[3].Intersects(sphere).HasFlag(PlaneIntersectionType.Intersecting))
            {
                    resolveNpcPlaneCollision(npc, planes[3]);
                //Console.WriteLine("RIGHT");
            }
        }


        // Resolve NPC/Plane collision
        // Adapted from book (Game Physics Engine Development)
        private void resolveNpcPlaneCollision(CollisionHandler npc, Plane plane)
        {
            // Get NPC bounding sphere
            BoundingSphere sphere = npc.getBoundingVolume();

            // Get sphere/plane distance (Hessian Normal Form)
            float dist = Vector3.Dot(plane.Normal, sphere.Center) + sphere.Radius + plane.D;

            //Console.WriteLine(dist);

            // Contact normal
            Vector3 velocity = npc.steer.velocity;
                velocity.Normalize();
                Vector3 normal;
                if (Vector3.Dot(plane.Normal, velocity) > 0)
                    normal = -plane.Normal;
                else
                    normal = plane.Normal;
           
            // Interpenetration depth
            float depth = Math.Abs(dist);

            // Get contact point
            Vector3 contact = sphere.Center - plane.Normal * (dist + sphere.Radius);


            // Get linear change in velocity
            float linearInertia = 1 / npc.steer.mass;

            // Get velocity along contact normal
            float contactVel1 = Vector3.Dot(npc.steer.velocity, normal);
            float closingVel = Math.Abs(contactVel1);
            float separatingVel = -closingVel;

            // Change in velocity, along normal
            separatingVel = -separatingVel * (1 + CollisionHandler.NPC_COEFF_OF_RESTITUTION);
            float impulse = separatingVel / linearInertia;
            Vector3 impulseVector = impulse * normal;

            // Impulse in world coordiantes
            Vector3 deltaVelocity = impulseVector / npc.steer.mass;

            // Apply impulse
            npc.applyImpulse(deltaVelocity);

            // Resolve interpenetration

            float linearInertia1 = 1 / npc.steer.mass;
            float linearInertia2 = 0;
            float totalInertia = linearInertia1 + linearInertia2;
            float linearMovement1 = depth * (linearInertia1 / totalInertia);
            Vector3 normalMove = linearMovement1 * normal;
            npc.applyLinearSeparation(normalMove);

        }

        // Resolve NPC/NPC collisions (two bounding spheres)
        public void resolveNpcNpcCollision(CollisionHandler npc1, CollisionHandler npc2)
        {
            // Get bounding spheres
            BoundingSphere bs1 = npc1.getBoundingVolume();
            BoundingSphere bs2 = npc2.getBoundingVolume();

            // Get min distance at which spheres are NOT colliding
            float minDist = bs1.Radius + bs2.Radius;

            // Get vector connecting sphere centers
            Vector3 midVector = bs1.Center - bs2.Center;

            // Get distance between spheres
            float dist = midVector.Length();

            // Get penetration depth
            float depth = minDist - dist;

            // Get sphere contact normal
            Vector3 normal = Vector3.Normalize(midVector);

            // Calculate contact point (midpoint of intersection)
            Vector3 contactPoint = bs1.Center + (midVector / 2);

            // Get linear change in velocity
            float linearInertia = 2 / npc1.steer.mass;

            // Get velocity along contact normal
            float contactVel1 = Vector3.Dot(npc1.steer.velocity, normal);
            float contactVel2 = Vector3.Dot(npc2.steer.velocity, normal);
            float closingVel = Math.Abs(contactVel1) + Math.Abs(contactVel2);
            float separatingVel = -closingVel;

            // Change in velocity, along normal
            separatingVel = separatingVel * (1 + CollisionHandler.NPC_COEFF_OF_RESTITUTION);
            float impulse = separatingVel / linearInertia;
            Vector3 impulseVector = impulse * normal;

            // Impulse in world coordiantes
            Vector3 deltaVelocity = impulseVector / npc1.steer.mass;

            // Apply impulse
            npc1.applyImpulse(deltaVelocity);
            npc2.applyImpulse(-deltaVelocity);

            // Resolve interpenetration

            float linearInertia1 = 1 / npc1.steer.mass;
            float linearInertia2 = 1 / npc2.steer.mass;
            float totalInertia = linearInertia1 + linearInertia2;
            float linearMovement1 = depth * (linearInertia1 / totalInertia);
            float linearMovement2 = -depth * (linearInertia2 / totalInertia);
            Vector3 normalMove = linearMovement1 * normal;
            Vector3 normalMove1 = linearMovement2 * normal;
            npc1.applyLinearSeparation(normalMove);
            npc2.applyLinearSeparation(normalMove1);

            //damage from collision with enemy
            if (npc1.type == CollisionHandler.Type.TEAMMATE && npc2.type == CollisionHandler.Type.ENEMY)
                npc1.steer.health -= 10;
            if (npc2.type == CollisionHandler.Type.TEAMMATE && npc1.type == CollisionHandler.Type.ENEMY)
                npc1.steer.health -= 10;
        }

        // Resolve intersections from parallel rays
        public Vector3 resolveIntersections(QuadTreeItem leftIntersection, QuadTreeItem rightIntersection, Ray leftRay, Ray rightRay)
        {
            if(leftIntersection != null && rightIntersection != null)
            {
                // Resolve the closest
                float? leftDist = leftRay.Intersects(leftIntersection.getBoundingVolume());
                float? rightDist = rightRay.Intersects(rightIntersection.getBoundingVolume());

                if(leftDist < rightDist)
                {
                    // Resolve left
                    return resolveSingleIntersection(leftIntersection, leftRay);
                }
                else
                {
                    // Resolve right
                    return resolveSingleIntersection(rightIntersection, rightRay);
                }
            }
            else if(leftIntersection != null)
            {
                // Resolve left
                return resolveSingleIntersection(leftIntersection, leftRay);
            }
            else if(rightIntersection != null)
            {
                // Resolve right
                return resolveSingleIntersection(rightIntersection, rightRay);
            }
            else
            {
                // They are both null, return anything
                return Vector3.Up;
            }
        }

        // Helper: resolve single-ray intersection
        private Vector3 resolveSingleIntersection(QuadTreeItem item, Ray ray)
        {
            // New Target for NPC
            Vector3 target = Vector3.Zero;

            // Get planes for each face of the item
            Plane[] faces = item.getFaces();

            // Find the nearest intersecting face, and its distance from the ray origin
            // Start from first (front) face
            Plane intersectingFace = faces[0];
            float? dist = ray.Intersects(intersectingFace);

            // Iterate over other faces
            for(int i = 1; i < faces.Length; ++i)
            {
                // If current (starting) face is null, replace it
                if(dist == null)
                {
                    intersectingFace = faces [i];
                    dist = ray.Intersects(faces[i]);
                }

                // ELSE, replace, if this face is intersecting closer
                else if(ray.Intersects(faces[i]) < dist)
                {
                    intersectingFace = faces [i];
                    dist = ray.Intersects(faces[i]);
                }
            }

            // Get surface normal for the face
            Vector3 normal = intersectingFace.Normal;
            //Vector3 normal = -ray.Direction;

            // Get intersection point
            Vector3 intersectionPoint = ray.Position + ray.Direction * (float)dist;

            // Calculate new target
            target = intersectionPoint + normal * CollisionHandler.WALL_AVOIDANCE_DIST;

            // Return new target
            return target;
        }
    }
         */
    }